/**
 * @file drv_lis2dh.c
 * @brief LIS2DH对接驱动
 * @author wangh (wanghuan3037@fiberhome.com)
 * @version 1.0
 * @date 2020-10-22
 * @copyright Copyright (c) 2020  烽火通信
 ***************************************************************
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version  <th>Author  <th>Description
 * <tr><td>2020-10-22 <td>1.0      <td>wangh   <td>内容
 * </table>
 */ 
#include "project_config.h"

#if !DRV_LIS2DH_EN
#include "drv_lis2dh.h"
#include "lis2dh_reg.h"
#include "bsp_i2c.h"

#include "project_board.h"
#undef LOG_LEVEL_
#define LOG_LEVEL_ 	LOG_LVL_ASSERT
#include "bsp_log.h"


static stmdev_ctx_t lis2dh_ctx;
#define LIS2DH_ADDR         0x19    ///< 从机地址
static I2c_t lis2dh_i2c_master;


/**
 * @brief I2C主机初始化
 * @return int8_t BSP_SUCCESS：成功
 */
static int8_t drv_lis2dh_iic_init(void)
{
    return I2cInit( &lis2dh_i2c_master, I2C_1, I2C1_SCL, I2C1_SDA );
}

/**
 * @brief lis2dh的i2c写移植接口
 * @param  handle       使用保留
 * @param  addr         寄存器地址
 * @param  pdata        写数据
 * @param  size         写数据长度
 * @return int32_t 0：成功
 */
static int32_t drv_lis2dh_port_iic_write(void *handle, uint8_t addr, uint8_t *pdata, uint16_t size)
{
    if(BSP_SUCCESS != I2cWriteBuffer( &lis2dh_i2c_master, LIS2DH_ADDR, addr, pdata, size ))
        return -1;

    return 0;
}

/**
 * @brief lis2dh的i2c读移植接口
 * @param  handle       使用保留
 * @param  addr         寄存器地址
 * @param  pdata        读数据
 * @param  size         读数据长度
 * @return int32_t 0：成功
 */
static int32_t drv_lis2dh_port_iic_read(void *handle, uint8_t addr, uint8_t *pdata, uint16_t size)
{
    if(BSP_SUCCESS != I2cReadBuffer( &lis2dh_i2c_master, LIS2DH_ADDR, addr, pdata, size ))
        return -1;

    return 0;
}

/**
 * @brief lis2dh初始化
 * @return int8_t BSP_SUCCESS：成功
 */
int8_t drv_lis2dh_init( void )
{
    int8_t ret = 0;
    lis2dh_ctx.write_reg = drv_lis2dh_port_iic_write;
    lis2dh_ctx.read_reg = drv_lis2dh_port_iic_read;

    /* i2c初始化 */
    ret = drv_lis2dh_iic_init();
    if(BSP_SUCCESS != ret)
    {
        log_e("drv_lis2dh_iic_init error(%d)!\r\n", ret);
        return -1;
    }

    uint8_t whoamI;
    lis2dh_device_id_get(&lis2dh_ctx, &whoamI);
    log_d("WHO_AM_I: 0X%02x\r\n", whoamI);
    if(whoamI != LIS2DH_ID)
    {
        log_e("WHO_AM_I get error !\r\n");
        return -1;
    }

    /* CTRL_REG1 设置采样频率，低功耗模式，开启x,y,z轴检测 */
    #if 1
    uint8_t data = 0x3f;
    lis2dh_write_reg(&lis2dh_ctx, LIS2DH_CTRL_REG1, &data, 1);
    data = 0x01;
    lis2dh_write_reg(&lis2dh_ctx, LIS2DH_CTRL_REG2, &data, 1);
    data = 0x80;
    lis2dh_write_reg(&lis2dh_ctx, LIS2DH_CTRL_REG4, &data, 1);
    #endif


    #if 0
    lis2dh_operating_mode_set(&lis2dh_ctx, LIS2DH_LP_8bit);
    lis2dh_data_rate_set(&lis2dh_ctx, LIS2DH_ODR_100Hz);
    lis2dh_axis_set(&lis2dh_ctx, LIS2DH_X_ENABLE|LIS2DH_Y_ENABLE|LIS2DH_Z_ENABLE);
    
    // CTRL_REG2 设置滤波器
    lis2dh_high_pass_on_outputs_set(&lis2dh_ctx, PROPERTY_ENABLE);
    lis2dh_high_pass_bandwidth_set(&lis2dh_ctx, LIS2DH_AGGRESSIVE);
    lis2dh_high_pass_mode_set(&lis2dh_ctx, LIS2DH_NORMAL_WITH_RST);
    lis2dh_high_pass_int_conf_set(&lis2dh_ctx, LIS2DH_ON_INT1_GEN);
    
    // CTRL_REG4 
    lis2dh_full_scale_set(&lis2dh_ctx, LIS2DH_2g);
    lis2dh_block_data_update_set(&lis2dh_ctx, PROPERTY_ENABLE);
    #endif


    #if (LIS2DH_USE_INT1_EN)
    // CTRL_REG3 中断引脚1配置
    lis2dh_ctrl_reg3_t ctrl_reg3;
    lis2dh_pin_int1_config_get(&lis2dh_ctx, &ctrl_reg3);
    ctrl_reg3.i1_ia1 = 1;	// AOI1 interrupt on INT1 pn
    lis2dh_pin_int1_config_set(&lis2dh_ctx, &ctrl_reg3);
    
    // INT1_CFG
    lis2dh_int1_cfg_t int1_cfg;
    int1_cfg.aoi = 0;
    int1_cfg._6d = 0;
    int1_cfg.xhie = 1;
    int1_cfg.xlie = 0;
    int1_cfg.yhie = 1;
    int1_cfg.ylie = 0;
    int1_cfg.zhie = 1;
    int1_cfg.zlie = 0;
    lis2dh_int1_gen_conf_set(&lis2dh_ctx, &int1_cfg);
    
    // INT1_THS
    lis2dh_int1_gen_threshold_set(&lis2dh_ctx, 32);	// 32 * 16mg@FS=2g
    
    // INT1_DURATION
    lis2dh_int1_gen_duration_set(&lis2dh_ctx, 1);	// 1/ODR
    #endif

    return 0;
}

/**
 * @brief 获取角度
 * @param  acc_dir     轴方向
 * @return float 角度值
 */
#include <math.h>
float drv_lis2dh_get_angle( ACC_DIR acc_dir )
{
    uint8_t acc_data[6];

    if(0 != lis2dh_acceleration_raw_get(&lis2dh_ctx, acc_data))
    {
        log_e("lis2dh_acceleration_raw_get error !\r\n");
        return -1;
    }

    log_d("x = %d, y = %d, z = %d\r\n", (*(int16_t*)acc_data), (*(int16_t*)(&acc_data[2])), (*(int16_t*)(&acc_data[4])));

    float acc_x = fabs(lis2dh_from_fs2_hr_to_mg(*(int16_t*)acc_data));
    float acc_y = fabs(lis2dh_from_fs2_hr_to_mg(*(int16_t*)(&acc_data[2])));
    float acc_z = fabs(lis2dh_from_fs2_hr_to_mg(*(int16_t*)(&acc_data[4])));

    log_d("acc_x = %d, acc_y = %d, acc_z = %d\r\n", acc_x, acc_y, acc_z);

    float acc_g = sqrt(pow(acc_x, 2) + pow(acc_y, 2) + pow(acc_z, 2));

    float angle, acc;
    if(acc_dir == ACC_DIR_X)
    {
        if (acc_x > acc_g)
            acc = acc_g;
        else
            acc = acc_x;
    }
    else if(acc_dir == ACC_DIR_Y)
    {
        if (acc_y > acc_g)
            acc = acc_g;
        else
            acc = acc_y;
    }
    else
    {
        if (acc_z > acc_g)
            acc = acc_g;
        else
            acc = acc_z;
    }

    angle = asin(acc/acc_g) * 2 / 3.14 * 90;
    angle = 90 - angle;
    if(angle < 0)
        angle = 0;

    return angle;
}

#endif	/* #if (DRV_LIS2DH_EN) */





/*********** (C) COPYRIGHT 2020 FiberHome *****END OF FILE****/
